/**
 * @author Yunfeng Bai
 *
 * 2010-11-12
 */
package simulation;

import java.util.ArrayList;
import java.util.List;

import javax.media.opengl.GL2;
import javax.vecmath.Quat4d;
import javax.vecmath.Vector3d;

import rigidbody.RigidBody;
import util.Timing;
import util.Util;
import collision.Collision;
import force.Force;

public class Simulator {
	private List<RigidBody> bodies;
	private List<Force> temporaryForces;
	private List<Force> persistentForces;
	private double timestep;
	
	private ConstraintSolver solver = new ProjectedGaussSeidelConstraintSolver();
	private CollisionDetector detector = new CollisionDetector();
	private boolean staticScene = false;
	
	public Simulator()
	{
		timestep = 1.0 / 60.0;
		bodies = new ArrayList<RigidBody>();
		temporaryForces = new ArrayList<Force>();
		persistentForces = new ArrayList<Force>();
	}

	public void addRigidBody(RigidBody body)
	{
		bodies.add(body);
		solver.rigidBodyAdded(this);
	}
	
	public void addTempForce(Force force)
	{
		temporaryForces.add(force);
	}
	
	public void addPersistantForce(Force force)
	{
		persistentForces.add(force);
	}
	
	public void removePersistantForce(Force force)
	{
		persistentForces.remove(force);
	}
	
	public void updateVelocities(boolean clearAccumulatedVars)
	{
		for (RigidBody body : bodies)
		{
			body.integrateVelocity(timestep);
			if (clearAccumulatedVars) body.clearAccumulatedVars();
		}
	}

	private void applyForces()
	{
		for (Force f : temporaryForces)
		{
			f.apply(timestep);
		}
		
		for (Force f : persistentForces)
		{
			f.apply(timestep);
		}
		
		temporaryForces.clear();
	}
	
	/**
	 * This method is the main method, which does the following:
	 * 1. Update bounding volume.
	 * 2. Apply forces.
	 * 3. Use velocity-based constraint solver to get the speed.
	 * 4. Integrate positions.
	 */
	public void update()
	{
		long time1, time2, time3, time4, time5, time6, time7;
		
		if (staticScene) return;
		
		time1 = System.nanoTime();
		
		// 1. Update bounding volumes.
		for (RigidBody body : bodies) body.updateOBBTree();
		
		time2 = System.nanoTime();
		
		// 2. Apply forces, set the torque and linear force.
		applyForces();
		
		time3 = System.nanoTime();
		
		// 3. Resolve velocities.
		solver.solveVelocities(this);
		
		// 4. Integrate positions.
		updatePositions();
		
		time7 = System.nanoTime();
		
//		Util.traceTime("Update bounding volume time ", (time2 - time1));
//		Util.traceTime("Apply forces time ", (time3 - time2));
//		Util.traceTime("Resolve collisions time ", (time4 - time3));
//		Util.traceTime("Updated velocities time ", (time5 - time4));
//		Util.traceTime("Resolve contacts time ", (time6 - time5));
//		Util.traceTime("Integrate positions time ", (time7 - time6));
		Util.trace("OBB collision test count " + Timing.OBBCollisionCount);
		Util.traceTime("OBB collision test time", Timing.OBBCollisionTime);
		Util.trace("ASDF get distance count " + Timing.ASDFGetDistanceCount);
		Util.traceTime("ASDF get distance time", Timing.ASDFGetDistanceTime);
		
		Timing.ASDFGetDistanceCount = 0;
		Timing.ASDFGetDistanceTime = 0;
		Timing.OBBCollisionCount = 0;
		Timing.OBBCollisionTime = 0;
		
		Timing.UpdateRigidBodyTime = time7 - time1;
		Util.traceTime("Update rigid body time ", (time7 - time1));
	}

	private void checkIfCollisionResolved()
	{
		for (int i = 0; i < bodies.size(); i++)
		{
			RigidBody b = bodies.get(i);
			b.backupPosition();
			b.integratePosition(timestep);
		}
		
		for (int i = 0; i < bodies.size(); i++)
		{
			RigidBody b1 = bodies.get(i);
			for (int j = 0; j < i; j++)
			{
				RigidBody b2 = bodies.get(j);
				List<Collision> collisions = RigidBody.getCollisionsBetween(b1, b2, true);
				if (collisions.size() > 0)
				{
					System.out.println("error");
				}
			}
		}
		
		for (int i = 0; i < bodies.size(); i++)
		{
			RigidBody b = bodies.get(i);
			b.restorePosition();
		}
	}

	private void updatePositions()
	{
		for (RigidBody body : bodies)
		{
			// For position.
			Vector3d dp = new Vector3d();
			dp.scale(timestep, body.getVelocity());
			body.getPosition().add(dp);
			
			// For orientation.
			Quat4d dq = new Quat4d();
			dq.scale(timestep, body.getDOrientation());
			body.getOrientation().add(dq);
			body.getOrientation().normalize();
		}
	}
	
	public void setStaticScene(boolean isStatic)
	{
		staticScene = isStatic;
	}
	
	public List<Force> getTemporaryForces()
	{
		return temporaryForces;
	}
	
	public List<Force> getPersistentForces()
	{
		return persistentForces;
	}
	
	public List<RigidBody> getRigidBodies()
	{
		return bodies;
	}
	
	public List<RigidBody> getBodies()
	{
		return bodies;
	}
	
	public double getTimestep()
	{
		return timestep;
	}
	
	public void draw(GL2 gl, boolean drawBoundingBox)
	{
		for (RigidBody body : bodies)
			body.draw(gl, drawBoundingBox);
	}
}
